// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceFunctionalTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Hardware.PicknPlace
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using Microsoft.Robotics.Hardware;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.PhysicalObjects;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Tests of the grasp, pose and sub task functionality in pick and place
    /// </summary>
    [TestClass]
    public class PicknPlaceFunctionalTests
    {
        /// <summary>
        /// Tolerance for comparing result and target orientations
        /// </summary>
        private const double OrientationTolerance = 0.01;

        /// <summary>
        /// Tolerance for comparing result and target positions
        /// </summary>
        private const double PositionTolerance = 0.01;

        /// <summary>
        /// Test the low level grasp and pose generation for an upright can using
        /// Kuka arm + Robotiq gripper
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationPicknPlaceKukaRobotiqGraspForCylinder()
        {
            // Upright cylinder
            PhysicalObjectDescription can = new PhysicalObjectDescription(PhysicalObjectShape.Cylinder, 0.075, 0.110, 0.075, 411.0, "canTest");
            PicknPlaceTaskDescriptionGenerator pickFrom = new PicknPlaceTaskDescriptionGenerator(new PicknPlaceKukaRobotiqGrip(), 1000);

            Pose objectPose = new Pose(new Vector3(-0.363, -0.471, can.HeightInM / 2.0));
            GripStyle gripStyle = GripStyle.WrapAroundPrincipalAxisFace1;
            Pose? expectedGripPose = new Pose(new Vector3(-0.3283, -0.4260, 0.0865), new Quaternion(0.5538, -0.4867, 0.0435, 0.6742));
            Pose? expectedPreGripPose = null;
            this.GripAndPoseTest(pickFrom, objectPose, can, gripStyle, expectedGripPose, expectedPreGripPose);

            gripStyle = GripStyle.TopDownAroundPrincipalAxis;
            expectedGripPose = new Pose(new Vector3(-0.3630, -0.4710, 0.1800), new Quaternion(0.7511, -0.6601, 0.0000, 0.0000));
            expectedPreGripPose = new Pose(new Vector3(-0.3630, -0.4710, 0.274), new Quaternion(0.7511, -0.6601, 0.0000, 0.0000));
            this.GripAndPoseTest(pickFrom, objectPose, can, gripStyle, expectedGripPose, expectedPreGripPose);

            objectPose = new Pose(new Vector3(-0.7560, -0.6360, can.HeightInM / 2.0));
            gripStyle = GripStyle.WrapAroundPrincipalAxisFace1;
            expectedGripPose = null;
            expectedPreGripPose = new Pose(new Vector3(-0.6150, -0.5174, 0.0865), new Quaternion(0.4984, -0.5433, -0.0290, 0.6750));
            this.GripAndPoseTest(pickFrom, objectPose, can, gripStyle, expectedGripPose, expectedPreGripPose);

            gripStyle = GripStyle.TopDownAroundPrincipalAxis;
            expectedGripPose = null;
            expectedPreGripPose = null;
            this.GripAndPoseTest(pickFrom, objectPose, can, gripStyle, expectedGripPose, expectedPreGripPose);
        }

        /// <summary>
        /// Test task step generation for can object
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationPickGeneratorTestForCan()
        {
            PicknPlaceTaskDescriptionGenerator generator = new PicknPlaceTaskDescriptionGenerator(new PicknPlaceKukaRobotiqGrip(), 1000);
            PhysicalObjectDescription can = PhysicalObjectDescription.CanTomato;
            List<MoveAndGraspTaskDescription> taskList;
            Pose startPose = new Pose(new Vector3(-0.68, 0.0, 0.15), new Quaternion(0, 0, 0, 1));
            Pose retractPose = new Pose(new Vector3(-0.6800, 0.000, 0.15), new Quaternion(0, 0, 0, 1));

            Pose gripExchangePose = new Pose(new Vector3(-0.45, 0.57, can.HeightInM / 2.0), new Quaternion(0.0, 0.0, 0.7285, -0.6851));
            double liftHeight = 0.14;

            Pose pickPose = new Pose(new Vector3(-0.436, -0.471, 0.000), new Vector3(0.0, 0.0, 0.0));
            int expectedStepCount = 8;
            List<GripStyle> gripStyles = new List<GripStyle> { GripStyle.TopDownAroundPrincipalAxis };
            taskList = generator.GetStepDescriptionsForPickTask(can, startPose, pickPose, retractPose, liftHeight);
            this.CheckSubTaskList(taskList, expectedStepCount, gripStyles);
            this.CheckGripperStates(taskList, GripStyle.TopDownAroundPrincipalAxis, 100.0, 5.0, 74.0, 10.0);

            Pose placePose = pickPose;
            taskList = generator.GetStepDescriptionsForPlaceTask(can, gripStyles[0], startPose, placePose, retractPose, liftHeight, true, gripExchangePose);
            this.CheckSubTaskList(taskList, expectedStepCount, gripStyles);

            placePose = new Pose(new Vector3(-0.73, -0.5, 0.00), new Vector3(0.0, 0.0, 0.0));
            expectedStepCount = 19;
            gripStyles.Add(GripStyle.WrapAroundPrincipalAxisFace1);
            taskList = generator.GetStepDescriptionsForPlaceTask(can, gripStyles[0], startPose, placePose, retractPose, liftHeight, true, gripExchangePose);
            this.CheckSubTaskList(taskList, expectedStepCount, gripStyles);
            this.CheckGripperStates(taskList, GripStyle.WrapAroundPrincipalAxisFace1, 100.0, 5.0, 50.0, 10.0);
        }

        /// <summary>
        /// Test task step generation for box object
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationPickGeneratorTestForBox()
        {
            PicknPlaceTaskDescriptionGenerator generator = new PicknPlaceTaskDescriptionGenerator(new PicknPlaceKukaRobotiqGrip(), 1000);
            PhysicalObjectDescription box = PhysicalObjectDescription.BoxSoap;

            List<MoveAndGraspTaskDescription> taskList;
            Pose startPose = new Pose(new Vector3(-0.68, 0.0, 0.15), new Quaternion(0, 0, 0, 1));
            Pose retractPose = new Pose(new Vector3(-0.6800, 0.000, 0.15), new Quaternion(0, 0, 0, 1));

            Pose gripExchangePose = new Pose(new Vector3(-0.45, 0.57, box.HeightInM / 2.0), new Quaternion(0.0, 0.0, 0.7285, -0.6851));
            double liftHeight = 0.14;

            Pose pickPose = new Pose(new Vector3(0.435, -0.499, -0.041), new Quaternion(0.0, 0.0, 0.7810, -0.6245));

            Pose placePose = new Pose(new Vector3(0.400, -0.534, -0.043), new Quaternion(0.0, 0.0, 0.6180, 0.7862));

            // Grip exchange
            pickPose = new Pose(new Vector3(-0.4315, -0.6997, -0.0207), new Quaternion(0.0, 0.0, 0.7285, -0.6851));
            placePose = new Pose(new Vector3(-0.2049, -0.5361, -0.0101), new Quaternion(0.0000, 0.0000, 0.6200, 0.7846));
            int expectedStepCount = 8;
            List<GripStyle> gripStyles = new List<GripStyle> { GripStyle.WrapAroundPrincipalAxisFace1, GripStyle.WrapAroundPrincipalAxisFace2 };
            taskList = generator.GetStepDescriptionsForPickTask(box, startPose, pickPose, retractPose, liftHeight);
            this.CheckSubTaskList(taskList, expectedStepCount, gripStyles);

            expectedStepCount = 19;
            taskList = generator.GetStepDescriptionsForPlaceTask(box, gripStyles[0], startPose, placePose, retractPose, liftHeight, true, gripExchangePose);
            gripStyles = new List<GripStyle> { GripStyle.WrapAroundPrincipalAxisFace2, GripStyle.TopDownAroundPrincipalAxis };
            this.CheckSubTaskList(taskList, expectedStepCount, gripStyles);
            this.CheckGripperStates(taskList, GripStyle.WrapAroundPrincipalAxisFace2, 100.0, 5.0, 70.0, 10.0);
            this.CheckGripperStates(taskList, GripStyle.TopDownAroundPrincipalAxis, 100.0, 5.0, 70.0, 10.0);
        }

        /// <summary>
        /// Test task step generation for bagel object using UR plus AX12 gripper
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationPickGeneratorTestForBagel()
        {
            PicknPlaceTaskDescriptionGenerator generator = new PicknPlaceTaskDescriptionGenerator(new PicknPlaceUr5Ax12DualGrip(), 1000);
            PhysicalObjectDescription bagel = PhysicalObjectDescription.Bagel;

            List<MoveAndGraspTaskDescription> taskList;
            double liftHeight = 0.14;
            Pose startPose = new Pose(new Vector3(-0.68, 0.0, 0.15), new Quaternion(0, 0, 0, 1));
            Pose retractPose = new Pose(new Vector3(-0.6800, 0.000, 0.15), new Quaternion(0, 0, 0, 1));

            Pose pickPose = new Pose(new Vector3(-0.436, -0.471, 0.015), new Vector3(0.0, 0.0, 0.0));
            int expectedStepCount = 8;
            List<GripStyle> gripStyles = new List<GripStyle> { GripStyle.TopDownAroundPrincipalAxis };
            taskList = generator.GetStepDescriptionsForPickTask(bagel, startPose, pickPose, retractPose, liftHeight);
            this.CheckSubTaskList(taskList, expectedStepCount, gripStyles);
            this.CheckGripperStates(taskList, GripStyle.TopDownAroundPrincipalAxis, 100.0, 5.0, 10.0, 10.0);
        }

        /// <summary>
        /// Validates gripper state in a task list
        /// </summary>
        /// <param name="taskList">Task list to check</param>
        /// <param name="style">grip style to check</param>
        /// <param name="expectOpen">Expected percentage for open state</param>
        /// <param name="openTol">Tolerance for open state</param>
        /// <param name="expectClose">Expected percentage for close state</param>
        /// <param name="closeTol">Tolerance for closed state</param>
        private void CheckGripperStates(List<MoveAndGraspTaskDescription> taskList, GripStyle style, double expectOpen, double openTol, double expectClose, double closeTol)
        {
            bool foundStyle = false;
            foreach (MoveAndGraspTaskDescription t in taskList)
            {
                GripperStatus grip = t.GripperState;
                if (grip.GripStyle == style)
                {
                    foundStyle = true;
                    if (grip.GripStateEnum == GripperStatus.GripperStateEnum.Open)
                    {
                        Assert.AreEqual(grip.PercentOpen, expectOpen, openTol);
                    }
                    else if (grip.GripStateEnum == GripperStatus.GripperStateEnum.Close)
                    {
                        Assert.AreEqual(grip.PercentOpen, expectClose, closeTol);
                    }
                }
            }

            Assert.IsTrue(foundStyle, "Did not find expected style");
        }

        /// <summary>
        /// Dumps a list
        /// </summary>
        /// <param name="taskList">dump list</param>
        private void DumpList(List<MoveAndGraspTaskDescription> taskList)
        {
            Console.WriteLine("Step Count {0}", taskList.Count);
            foreach (MoveAndGraspTaskDescription t in taskList)
            {
                Vector3 v = t.ArmPose.Position;
                Quaternion q = t.ArmPose.Orientation;
                Console.Write(t.Description);
                Console.Write(" {0} [ {1:F4} {2:F4} {3:F4} ]", t.GripperState.GripStyle.ToString(), v.X, v.Y, v.Z);
                Console.WriteLine("\t[ {0:F4} {1:F4} {2:F4} {3:F4} ]", q.W, q.X, q.Y, q.Z);
            }
        }

        /// <summary>
        /// Test grip and pose generation
        /// </summary>
        /// <param name="generator">The pick and place generator interface</param>
        /// <param name="objectPose">Pose of object</param>
        /// <param name="objectDescription">Object description</param>
        /// <param name="gripStyle">Grip style to use</param>
        /// <param name="expectedGripPose">Expected arm pose for the grip, null if expect no pose to be generated</param>
        /// <param name="expectedPreGripPose">Expected arm pose for the pre grip, null if expect no pose to be generated</param>
        private void GripAndPoseTest(
            PicknPlaceTaskDescriptionGenerator generator,
            Pose objectPose,
            PhysicalObjectDescription objectDescription,
            GripStyle gripStyle,
            Pose? expectedGripPose,
            Pose? expectedPreGripPose)
        {
            IPicknPlaceObjectGrip objectGrip = PicknPlaceObjectGrip.ObjectGripFactory(objectDescription);

            GripState grip = generator.GripFromPickAndGripStyle(objectGrip, objectPose, gripStyle);
            GripState preGrip = generator.PreGripFromGrip(grip);

            try
            {
                double score;
                double surfaceOffset = objectGrip.GetHorizontalPlaneOffset(objectPose);
                Pose grasp = generator.ArmPoseFromGrip(grip, surfaceOffset, out score);
                Assert.IsTrue(score > 0.0);
                Assert.IsNotNull(expectedGripPose, string.Format("Expected a valid grip pose for object position {0} {1} {2}", objectPose.Position.X, objectPose.Position.Y, objectPose.Position.Z));
                this.CheckPoseMatch(grasp, (Pose)expectedGripPose);
            }
            catch (ArgumentException)
            {
                Assert.IsNull(expectedGripPose, string.Format("Unexpected non grip pose result for {0} {1} {2}", objectPose.Position.X, objectPose.Position.Y, objectPose.Position.Z));
            }

            try
            {
                double score;
                double surfaceOffset = objectGrip.GetHorizontalPlaneOffset(objectPose);
                Pose preGrasp = generator.ArmPoseFromGrip(preGrip, surfaceOffset, out score);
                Assert.IsTrue(score > 0.0);
                Assert.IsNotNull(expectedPreGripPose, string.Format("Expected a valid pre grip pose for object position {0} {1} {2}", objectPose.Position.X, objectPose.Position.Y, objectPose.Position.Z));
                this.CheckPoseMatch(preGrasp, (Pose)expectedPreGripPose);
            }
            catch (ArgumentException)
            {
                Assert.IsNull(expectedPreGripPose, string.Format("Unexpected non pre grip pose result for {0} {1} {2}", objectPose.Position.X, objectPose.Position.Y, objectPose.Position.Z));
            }
        }

        /// <summary>
        /// Runs sanity checks on a sub task list
        /// </summary>
        /// <param name="taskList">Task list to check</param>
        /// <param name="expectedStepCount">Expected number of steps</param>
        /// <param name="expectedGripStyles">Expected grip styles (exhaustive list)</param>
        private void CheckSubTaskList(List<MoveAndGraspTaskDescription> taskList, int expectedStepCount, List<GripStyle> expectedGripStyles)
        {
            Dictionary<GripStyle, bool> isGripStyleFound = new Dictionary<GripStyle, bool>();

            Assert.IsTrue(expectedStepCount == taskList.Count, string.Format("Expected {0} task steps Found {1}", expectedStepCount, taskList.Count));

            expectedGripStyles.ForEach(s => isGripStyleFound.Add(s, false));
            taskList.ForEach(
                t =>
                {
                    Assert.IsTrue(isGripStyleFound.Keys.Contains(t.GripperState.GripStyle), string.Format("Found unexpected grip state {0}", t.GripperState.GripStyle.ToString()));
                    isGripStyleFound[t.GripperState.GripStyle] = true;
                });

            foreach (KeyValuePair<GripStyle, bool> kvp in isGripStyleFound)
            {
                Assert.IsTrue(kvp.Value, string.Format("Did not find grip style {0}", kvp.Key.ToString()));
            }
        }
            
        /// <summary>
        /// Checks if two pose match within a tolerance
        /// </summary>
        /// <param name="result">Result pose</param>
        /// <param name="expected">Expected pose</param>
        private void CheckPoseMatch(Pose result, Pose expected)
        {
            double quatDiff = Quaternion.Length(result.Orientation - expected.Orientation);
            double posDiff = result.Position.DistanceFrom(expected.Position);
            bool ready = quatDiff < PicknPlaceFunctionalTests.OrientationTolerance &&
                posDiff < PicknPlaceFunctionalTests.PositionTolerance;

            if (quatDiff > PicknPlaceFunctionalTests.OrientationTolerance && Math.Abs(expected.Orientation.W) < 0.001)
            {
                // Corner case check for equivalent rotations 
                double equivThreshold = PicknPlaceFunctionalTests.OrientationTolerance / 4.0;
                if (Math.Abs(Math.Abs(expected.Orientation.X) - Math.Abs(result.Orientation.X)) < equivThreshold &&
                    Math.Abs(Math.Abs(expected.Orientation.Y) - Math.Abs(result.Orientation.Y)) < equivThreshold &&
                    Math.Abs(Math.Abs(expected.Orientation.Z) - Math.Abs(result.Orientation.Z)) < equivThreshold &&
                    Math.Abs(Math.Abs(expected.Orientation.W) - Math.Abs(result.Orientation.W)) < equivThreshold)
                {
                    ready = posDiff < PicknPlaceFunctionalTests.PositionTolerance;
                }
            }

            Assert.IsTrue(
                ready,
                string.Format(
                "Result pose {0} {1} {2} does not match expected pose {3} {4} {5}",
                result.Position.X,
                result.Position.Y,
                result.Position.Z,
                expected.Position.X,
                expected.Position.Y,
                expected.Position.Z));
        }
    }
}
